#include "mainwindow.h"
#include "ui_mainwindow.h"

// 旋转方向，初始为顺时针
int dr = 0;
float input_pos;
auto input_pos_data = std_msgs::msg::Float32();
auto input_vel_data = std_msgs::msg::Float32();
auto flag = std_msgs::msg::Int32();


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    // 设置文本框默认值
    ui->speed_text_cb->setPlaceholderText("0");
    ui->voltage_text_cb->setPlaceholderText("0");
    ui->voltage_text_cb_2->setPlaceholderText("0");

    ui->pushButton_2->setCheckable(true);
    ui->levelCalibration->setCheckable(true);
    ui->warningButton->setCheckable(true);



    // 实例化Thread构造函数
    driverThread = new DriverThread(this);
    voltageWheelThread = new VoltageWheelThread(this);
    voltageArmThread = new VoltageArmThread(this);
    // imu
    imuDataThread = new ImuDataThread(this);
    imuDataThread->start();
    // speed反馈
    speedDataThread = new SpeedDataThread(this);
    speedDataThread->start();
    // camera_1
    camera1DataThread = new Camera1DataThread(this);
    camera1DataThread->start();
    // camera_2
    camera2DataThread = new Camera2DataThread(this);
    camera2DataThread->start();
    // camera_3
    camera3DataThread = new Camera3DataThread(this);
    camera3DataThread->start();
    // 拉力传感器
    weighDataThread = new WeighDataThread(this);
    weighDataThread->start();
    // 温度传感器
    temDataThread = new TemDataThread(this);
    temDataThread->start();


    // 实例化Qprocess构造函数
    keyboard = new QProcess(this);
    driver = new QProcess(this);

    // imu信号到一个槽函数
    connect(imuDataThread,&ImuDataThread::imuEmitData,this,&MainWindow::display_imu);
    // weigh信号
    connect(weighDataThread,&WeighDataThread::weigh1_emit,this,&MainWindow::display_weigh1);
    connect(weighDataThread,&WeighDataThread::weigh2_emit,this,&MainWindow::display_weigh2);
    // 速度反馈
    connect(speedDataThread,&SpeedDataThread::speedEmitData1,this,&MainWindow::displayrt1data);
    connect(speedDataThread,&SpeedDataThread::speedEmitData2,this,&MainWindow::displayrt2data);
    connect(speedDataThread,&SpeedDataThread::speedEmitData3,this,&MainWindow::displayrt3data);
    connect(speedDataThread,&SpeedDataThread::speedEmitData4,this,&MainWindow::displayrt4data);
    // 温度反馈
    connect(temDataThread,&TemDataThread::tem_emit,this,&MainWindow::display_tem);
    // 漆膜仪反馈
     connect(temDataThread,&TemDataThread::thickness_emit,this,&MainWindow::display_thick);
    // camera1信号到一个槽函数
    connect(camera1DataThread,&Camera1DataThread::image_1Emit,this,&MainWindow::display_camera1,Qt::BlockingQueuedConnection);
    // camera2信号到一个槽函数
    connect(camera2DataThread,&Camera2DataThread::image_2Emit,this,&MainWindow::display_camera2,Qt::BlockingQueuedConnection);
    // camera3信号到一个槽函数
    connect(camera3DataThread,&Camera3DataThread::image_3Emit,this,&MainWindow::display_camera3,Qt::BlockingQueuedConnection);
    // // 测试label显示图片
    // QImage *img = new QImage;
    // img->load("/home/zhangchenxu/Pictures/摄像头/01.jpg");
    // ui->camera1Label->setPixmap(QPixmap::fromImage(*img));

}

MainWindow::~MainWindow()
{

    rclcpp::shutdown();
    delete ui;

}

void MainWindow::display_tem(int tem)
{
    // 设置输出框无光标且只读
    ui->tem_play->setStyleSheet("QLineEdit {}");
    ui->tem_play->setReadOnly(true);
    ui->tem_play->setText(QString::number(tem));
}

void MainWindow::display_thick(int thick)
{
    ui->thick_play->setStyleSheet("QLineEdit {}");
    ui->thick_play->setReadOnly(true);
    ui->thick_play->setText(QString::number(thick));
}



void MainWindow::displayrt1data(float turespeed1)
{
    // 设置输出框无光标且只读
    ui->lf_speed->setStyleSheet("QLineEdit {}");
    ui->lf_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(2) << turespeed1;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->lf_speed->setText(QString::number(rt1));
}
void MainWindow::displayrt2data(float turespeed2)
{
    // 设置输出框无光标且只读
    ui->rf_speed->setStyleSheet("QLineEdit {}");
    ui->rf_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(2) << turespeed2;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->rf_speed->setText(QString::number(rt1));
}
void MainWindow::displayrt3data(float turespeed3)
{
    // 设置输出框无光标且只读
    ui->lb_speed->setStyleSheet("QLineEdit {}");
    ui->lb_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(2) << turespeed3;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->lb_speed->setText(QString::number(rt1));
}
void MainWindow::displayrt4data(float turespeed4)
{
    // 设置输出框无光标且只读
    ui->rb_speed->setStyleSheet("QLineEdit {}");
    ui->rb_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(2) << turespeed4;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->rb_speed->setText(QString::number(rt1));
}

// void MainWindow::displaytem(int tem)
// {
//      qDebug()<<"voltageValue";
// }

// 小车底盘速度调整滑块
void MainWindow::on_speedSlider_valueChanged(int value)
{
    // 调用driverThread中的setDriverValue方法，将value的值传入
    driverThread->setDriverValue(value);
    // 转换数值，使其显示在ui上
    float float_value = (float)value / 100 * 12;
    ui->speed_text_cb->setText(QString::number(float_value, 'f', 2));
    // 开启线程
    driverThread->start();
}



// 辅助轮电推杆电压控制滑块
// void MainWindow::on_voltageSlider_valueChanged(int value)
// {
//     // 调用voltageWheelThread中的setVoltageValue方法，将value的值传入
//     voltageWheelThread->setVoltageValue(value);
//     // 使数值显示在UI上
//     ui->voltage_text_cb->setText(QString::number(value));
//     // 开启线程
//     voltageWheelThread->start();
// }
// 辅助轮按钮发布数据
// 抬升
void MainWindow::on_pushButton_clicked()
{
    float voltageValue = ui->setpushvalue->text().toFloat();
    if (voltageValue < 0)
    {
        voltageValue = - voltageValue;
    }
    voltageWheelThread->setVoltageValue(voltageValue);
    qDebug()<<voltageValue;
    // 使数值显示在UI上
    ui->voltage_text_cb->setText(QString::number(voltageValue));
    // 开启线程
    voltageWheelThread->start();
}
// 下降
void MainWindow::on_downButton_clicked()
{
    float voltageValue = ui->setpushvalue->text().toFloat();
    if (voltageValue > 0)
    {
        voltageValue = - voltageValue;
    }
    voltageWheelThread->setVoltageValue(voltageValue);
    qDebug()<<voltageValue;
    // 使数值显示在UI上
    ui->voltage_text_cb->setText(QString::number(voltageValue));
    // 开启线程
    voltageWheelThread->start();
}
// 停止
void MainWindow::on_stopButton_clicked()
{
    float voltageValue = 0 ;
    voltageWheelThread->setVoltageValue(voltageValue);
    qDebug()<<voltageValue;
    // 使数值显示在UI上
    ui->voltage_text_cb->setText(QString::number(voltageValue));
    // 开启线程
    voltageWheelThread->start();
}



// 前臂推杆电压值控制滑块
void MainWindow::on_horizontalSlider_valueChanged(int value)
{
    // 调用set方法，将值传入
    voltageArmThread->setVoltageValue(value);
    // 使数值显示在UI上
    ui->voltage_text_cb_2->setText(QString::number(value));
    // 开启线程
    voltageArmThread->start();
}

void MainWindow::on_twistKeyboard_clicked()
{
    keyboard->start("terminator",
                    QStringList()<< "-e" <<
                        "ros2 run teleop_twist_keyboard teleop_twist_keyboard");
}


// void MainWindow::on_driverButton_clicked()
// {
//     driver->start("terminator",
//                   QStringList()<< "-e" <<
//                       "cd Documents/Wall-climbing/ros_driver && . install/setup.bash && ros2 run ros_driver ros_can_driver_node");
// }

// void MainWindow::on_wheelPushButton_clicked()
// {
//     wheel->start("terminator",
//                  QStringList()<< "-e" <<
//                      "cd Documents/Wall-climbing/ros_to_32 && . install/setup.bash && ros2 run ros_to_stm32 lis");
// }

void MainWindow::display_imu(double dis_r,double dis_p,double dis_y)
{
    // 设置imu的输出框无光标且只读
    ui->rollDisplay->setStyleSheet("QLineEdit {}");
    ui->rollDisplay->setReadOnly(true);
    ui->pitchDisplay->setStyleSheet("QLineEdit {}");
    ui->pitchDisplay->setReadOnly(true);
    ui->yawDisplay->setStyleSheet("QLineEdit {}");
    ui->yawDisplay->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_r, ss_p, ss_y;
    ss_r << std::fixed << std::setprecision(2) << dis_r;
    ss_p << std::fixed << std::setprecision(2) << dis_p;
    ss_y << std::fixed << std::setprecision(2) << dis_y;

    float r = std::stof(ss_r.str());
    float p = std::stof(ss_p.str());
    float y = std::stof(ss_y.str());
    // 显示在UI中
    ui->rollDisplay->setText(QString::number(r));
    ui->pitchDisplay->setText(QString::number(p));
    ui->yawDisplay->setText(QString::number(y));
}

void MainWindow::display_weigh1(float dis_w1)
{
    ui->l_push->setStyleSheet("QLineEdit {}");
    ui->l_push->setReadOnly(true);
    // 显示在UI中
    ui->l_push->setText(QString::number(dis_w1,'f',1));
}
void MainWindow::display_weigh2(float dis_w2)
{
    ui->r_push->setStyleSheet("QLineEdit {}");
    ui->r_push->setReadOnly(true);
    // 显示在UI中
    ui->r_push->setText(QString::number(dis_w2,'f',1));
}


void MainWindow::display_camera1(cv::Mat img1)
{
    cv::Mat rgb1;
    QImage qimg1;
    cv::cvtColor(img1, rgb1, CV_BGR2RGB);
    qimg1 = QImage((const unsigned char*)(rgb1.data),
                   rgb1.cols, rgb1.rows,
                   rgb1.cols*rgb1.channels(),
                   QImage::Format_RGB888);

    ui->camera1Label->setPixmap(QPixmap::fromImage(qimg1));
    // ui->camera1Label->setScaledContents(true); //开启后无法正常显示？

}

void MainWindow::display_camera2(cv::Mat img2)
{
    cv::Mat rgb2;
    QImage qimg2;
    cv::cvtColor(img2, rgb2, CV_BGR2RGB);
    qimg2 = QImage((const unsigned char*)(rgb2.data),
                   rgb2.cols, rgb2.rows,
                   rgb2.cols*rgb2.channels(),
                   QImage::Format_RGB888);

    ui->camera2Label->setPixmap(QPixmap::fromImage(qimg2));
}
void MainWindow::display_camera3(cv::Mat img3)
{
    cv::Mat rgb3;
    QImage qimg3;
    cv::cvtColor(img3, rgb3, CV_BGR2RGB);
    qimg3 = QImage((const unsigned char*)(rgb3.data),
                   rgb3.cols, rgb3.rows,
                   rgb3.cols*rgb3.channels(),
                   QImage::Format_RGB888);

    ui->camera3Label->setPixmap(QPixmap::fromImage(qimg3));
}



// 以下均是改变机械臂方向的实现
void MainWindow::on_clockwise_clicked()
{
    // 如果点击的是正转按钮，则赋0
    dr = 0;
}

void MainWindow::on_c_clockwise_clicked()
{
    // 如果点击反转按钮，则赋1
    dr = 1;
}
// 正前
void MainWindow::on_formerButton_clicked()
{

    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!pos_pub_)
    {
        pos_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("pos_true", 10);
    }
    if(dr == 0)
    {
        input_pos = 0;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
    if(dr == 1)
    {
        input_pos = -0;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
}


void MainWindow::on_rightButton_clicked()
{
    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!pos_pub_)
    {
        pos_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("pos_true", 10);
    }
    if(dr == 0)
    {
        input_pos = 100;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
    if(dr == 1)
    {
        input_pos = -260;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }

}

void MainWindow::on_backButton_clicked()
{
    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!pos_pub_)
    {
        pos_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("pos_true", 10);
    }
    if(dr == 0)
    {
        input_pos = 180;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
    if(dr == 1)
    {
        input_pos = -180;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
}




void MainWindow::on_leftButton_clicked()
{
    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!pos_pub_)
    {
        pos_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("pos_true", 10);
    }
    if(dr == 0)
    {
        input_pos = 260;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
    if(dr == 1)
    {
        input_pos = -100;
        input_pos_data.data = input_pos;
        // 发布数据
        pos_pub_->publish(input_pos_data);
    }
}


void MainWindow::on_removeBu1_clicked()
{
    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!vel_pub_)
    {
        vel_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("vel_true", 10);
    }
    float data = 100 ;
    input_vel_data.data = data ;
    vel_pub_->publish(input_vel_data);
}
void MainWindow::on_removeBu2_clicked()
{
    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!vel_pub_)
    {
        vel_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("vel_true", 10);
    }
    float data = - 100 ;
    input_vel_data.data = data ;
    vel_pub_->publish(input_vel_data);
}
void MainWindow::on_stopBu_clicked()
{
    // 防止重复定义
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!vel_pub_)
    {
        vel_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Float32>("vel_true", 10);
    }
    float data = 0 ;
    input_vel_data.data = data ;
    vel_pub_->publish(input_vel_data);
}

void MainWindow::on_pushButton_2_toggled(bool checked)
{
    // 防止重复定义,借用节点
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!flag_pub_)
    {
        flag_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Int32>("mode_flag",2);
    }
    if (checked) {
        // 如果按钮被按下（checked 为 true），输出 1
        flag.data = 1;
        flag_pub_->publish(flag);
        qDebug() << "1";
    } else {
        // 如果按钮被释放（checked 为 false），输出 0
        flag.data = 0;
        flag_pub_->publish(flag);
        qDebug() << "0";
    }
}


void MainWindow::on_levelCalibration_toggled(bool checked)
{
    // 防止重复定义,借用节点
    if(!arm_talker_node_cpp)
    {
        arm_talker_node_cpp = rclcpp::Node::make_shared("Arm_Talker");
    }
    if(!flag_pub_)
    {
        flag_pub_ = arm_talker_node_cpp->create_publisher<std_msgs::msg::Int32>("flag",2);
    }
    if (checked) {
        // 如果按钮被按下（checked 为 true），输出 1
        flag.data = 2;
        flag_pub_->publish(flag);
        qDebug() << "2";
    } else {
        // 如果按钮被释放（checked 为 false），输出 0
        flag.data = 0;
        flag_pub_->publish(flag);
        qDebug() << "0";
    }
}







